/*
 * lowpass_filter.c
 *
 *  Created on: Nov 20, 2024
 *      Author: Vinson
 */

#include "micros.h"
#include "lowpass_filter.h"
#include "stdlib.h"
#include "gpio.h"
extern gpio_t dbg_out2;

/* @brief  ：低通滤波器
 * @name   ：lowpass_filter_calculate
 * @param  ：x ， 要滤波的原始值
 * @return ： 返回滤波后的值
 * @details：y  = alpha * y_prev + (1 - alpha) * x
 */
static float lowpass_filter_calculate(lowpass_filter_t * filter, float x)
{
	float y = x;
	uint64_t ts_prev, ts_now;
	float tf = 0.0f;
	//gpio_set_value(&dbg_out2, 0);
	assert_param(filter);

	ts_prev = filter->ts_prev;
	tf = filter->tf;
	ts_now = micros();

    float dt = (ts_now - ts_prev) * 1e-6f;

	if (dt > 0.3f) {
		goto filter_done;
	}

	if (dt < 0.0f)
		dt = 1e-3f;

	/*
	 * y  = alpha * y_prev + (1 - alpha) * x
	 */
	float alpha = tf / (tf + dt);
	y = alpha * filter->output_prev + (1.0f - alpha) * x;

	//printf("dt = %f, tf = %f, alpha = %f, x = %f, y = %f, output_prev = %f\n", dt, tf, alpha, x, y, filter->output_prev);

filter_done:
	filter->output_prev = y;
	filter->ts_prev = ts_now;
	//gpio_set_value(&dbg_out2, 1);

	return y;
}

/* @brief  ：低通滤波器初始化
 * @name   ：lowpass_filter_init
 * @param  ：tf。滤波器tf 参数
 * @return ： 无
 * @details：无
 */
static void lowpass_filter_init(lowpass_filter_t *filter, float tf)
{
	assert_param(filter);
	filter->tf = tf;
	filter->output_prev = 0.0f;
	filter->ts_prev = micros();
}

/* @brief  ：创建一个低通滤波器
 * @name   ：lowpass_filter_create
 * @param  ：tf。滤波器tf 参数
 * @return ： 返回滤波器指针
 * @details：无
 */
lowpass_filter_t *lowpass_filter_create(float tf)
{
	lowpass_filter_t *filter = (lowpass_filter_t *)malloc(sizeof(lowpass_filter_t));
	assert_param(filter);

	filter->init = lowpass_filter_init;
	filter->lpf = lowpass_filter_calculate;

	filter->init(filter, tf);

	return filter;
}

/* @brief  ：滤波器销毁
 * @name   ：lowpass_filter_destory
 * @param  ：filter。滤波器对象
 * @return ： 无
 * @details：无
 */
void lowpass_filter_destory(lowpass_filter_t *filter)
{
	assert_param(filter);
	free((void *)filter);
	filter = NULL;
}
